#include <iostream>
#include <set>
#include <ctime>
#include <map>
#include <limits>
#include <vector>
#include <list>
#include <stack>
#include <math.h>

using namespace std;

struct Vertex {
public:
    double x;
    double y;
    bool wasVisited;
    double cost;
    
    Vertex() {
        x=0;
        y=0;
        wasVisited=false;
        cost=0;
    }
    
    double getLength(Vertex* other) {
        return sqrt( (x - other->x)*(x - other->x) + (y - other->y)*(y - other->y) );
    }
};

void generateGraph(vector<Vertex*>& g, int S1, int S2, int N, int M) {
    vector<Vertex> firstTowns(N);
    
    for (int i = 0; i < N; ++i) {
        double d1 = 1.0 - (double)(rand()) / (RAND_MAX + 1);
        double COS = cos(2 * 3.14 * d1);
        double LOG = log(d1);
        
        firstTowns[i].x = S1 * COS * sqrt(-2 * LOG);
        firstTowns[i].y = S2 * sqrt(1.0 - COS * COS) * sqrt(-2 * LOG);
    }

    for (int i = 0; i < N; ++i) {
        for (int j = 0; j < M; ++j) {
            double d2 = 1.0 - (double)(rand()) / (RAND_MAX + 1);
            double COS = cos(2 * 3.14 * d2);
            double LOG = log(d2);

            Vertex* newTown = new Vertex();
            newTown->x=S2 * COS * sqrt(-2 * LOG) + firstTowns[i].x;
            newTown->y=S2 * sqrt(1 - COS * COS) * sqrt(-2 * LOG) + firstTowns[i].y;

            g[i * M + j]=newTown;
        }
    }
}

void primaAlgorithm(vector<Vertex*>& g, map<int, list<int> >& mst, vector<int>& pi, int N, int M) {
    for (size_t i = 0; i != g.size(); ++i) {
        pi[i] = -1;
        g[i]->cost = std::numeric_limits<double>::infinity();
        g[i]->wasVisited = false;
    }
    
    int start = 0;
    g[start]->cost = 0;

    std::multimap<double, int> q;
    for (size_t i = 0; i<g.size(); ++i){
        q.insert(make_pair(g[i]->cost, i));
    }

    size_t v, u;

    while (!q.empty()) {
        v = q.begin()->second;
        q.erase(q.begin());
        g[v]->wasVisited = true;

        for (size_t i = 0; i < g.size(); ++i) {
            if (g[i]->wasVisited) {
                continue;
            }

            for (std::multimap<double, int>::iterator j=q.lower_bound(g[i]->cost); j!=q.upper_bound(g[i]->cost); ++j) {
                if (j->second == i) {
                    break;
                }
            }

            double distanse =  g[v]->getLength(g[i]);
            if (distanse <g[i]->cost) {
                q.erase(q.lower_bound(g[i]->cost));
                g[i]->cost = distanse;
                pi[i] = v;
                q.insert(make_pair(distanse, i));
            }
        }

        if (pi[v] != -1 && g[v] != NULL) {
            mst[pi[v]].push_back(v);
            mst[v].push_back(pi[v]);
        }
    }
}

void findEilerPath(vector<Vertex*>& g, map<int, list<int> >& mst, vector<int>& way, int N, int M) {
    stack<int> stc;
    int start = mst.begin()->first;
    set<pair<int, int>  > closed;
    
    stc.push(start);
    while (!stc.empty()) {
        int v = stc.top();
        bool allClosed = true;
        for (list<int>::iterator i = mst[v].begin(); i != mst[v].end(); ++i) {
            pair<int, int> pr = make_pair(v, *i);
            if (closed.find(pr) == closed.end()) {
                allClosed = false;
                closed.insert(pr);
                stc.push(*i);
            }
        }
        if (allClosed) {
            way.push_back(v);
            stc.pop();
        }
    }
}

double woodenAlgorithm(vector<Vertex*>& g, vector<int>& newWay, vector<int>& pi, int N, int M) {
    double summ = 0;
    
    map<int, list<int> > mst;
    primaAlgorithm(g, mst, pi, N, M);

    vector<int> way;
    findEilerPath(g, mst, way, M, N);
    for (size_t i = 0;  i<way.size(); ++i) {
        g[way[i]]->wasVisited = false;
    }

    int old, next = way[0];
    newWay.push_back(next);
    g[next]->wasVisited = true;
    for (size_t i = 1;  i<way.size(); ++i) {
        old = next;
        while (i<way.size()  && g[way[i]]->wasVisited) {
            ++i;
        }
        if (i>=way.size()) {
            break;
        }
        next = way[i];
        g[next]->wasVisited = true;
        newWay.push_back(next);
    }

    old = newWay[0];
    for(int y = 1; y<newWay.size(); ++y) {
         summ += g[old]->getLength(g[newWay[y]]);
         old = newWay[y];
    }
    summ += g[newWay[newWay.size()-1]]->getLength(g[newWay[0]]);
    return summ;
}


double closestNeighbourAlgorithm(vector<Vertex*>& g, vector<int>& pi, int N, int M) {
    vector<int> way;
    for (size_t i = 0; i<g.size(); ++i) {
        g[i]->wasVisited = false;
        pi[i] = -1;
    }

    int start = 0;
    int cur = 0;

    double sum = 0;
    int old = start;
    for (size_t i = 0; i<g.size()-1; ++i) {
        g[cur]->wasVisited = true;
        way.push_back(cur);
        sum += g[old]->getLength(g[cur]);
        old = cur;

        double min = numeric_limits<double>::max();
        int minVertex = -1;

        for (int j = 0; j < g.size(); ++j) {
            if (!g[j]->wasVisited) {
                double distance = g[cur]->getLength(g[j]);
                if (distance < min) {
                    min = distance;
                    minVertex = j;
                }
            }
        }
        pi[minVertex] = cur;
        cur = minVertex;
    }
    pi[start] = cur;
    way.push_back(cur);
    sum += g[old]->getLength(g[cur]);
    sum += g[way[way.size()-1]]->getLength(g[way[0]]);

    return sum;
}

int main() {
    int S1 = 100;
    int S2 = 10;
    int N = 10;
    int M = 10;

    srand(time(NULL));

    vector<Vertex*> g(N * M);
    generateGraph(g, S1, S2, N, M);

    vector<int> pi(N*M, NULL);

    clock_t t = clock();
    vector<int> way;
    cout<<"Wooden algorithm: "<<woodenAlgorithm(g, way, pi, N, M);
    cout<<" ("<<((double)((clock() - t)) / CLOCKS_PER_SEC)<<" sec)"<<endl;
    t = clock();
    cout<<"Closest neighbour: "<<closestNeighbourAlgorithm(g, pi, N, M);
    cout<<" ("<<((double)((clock() - t)) / CLOCKS_PER_SEC)<<" sec)"<<endl;

    system("pause");
    return 0;
}